#pragma once
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/iterative_spline_parameterization.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <iostream>

#include <std_msgs/Int32.h>
#include "tf/LinearMath/Quaternion.h"
#include "../include/arm_moveit_kinematics/key_type.h"
#include "../include/arm_moveit_kinematics/cope_yaml.h"
#include "arm_moveit_kinematics/arm_data.h"

using namespace key_ctrl;
namespace rvt = rviz_visual_tools;

typedef   enum ActionModeEnumdef
{
        /* data */
        front_point,
        init_point,
        back_point
}ActionModeEnumdef;
      
class arm_allfunction
{
private:
        /* data */
        cope_yaml get_yaml_info;
        
        geometry_msgs::Pose pose;
        const   std::string groupName;
        const   std::string baseframeName; 

        moveit::planning_interface::MoveGroupInterface   group;
        moveit_visual_tools::MoveItVisualTools visual_tools;
        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
        moveit::planning_interface::MoveGroupInterface::Plan multy_plan;


        ros::NodeHandle allfunc_node;
        ros::Subscriber joint_state_sub;
        ros::Subscriber key_ctrl_sub;
        ros::Publisher joint_data_pub;

        sensor_msgs::JointState joint_state_msg;
        std_msgs::Int32 key_ctrl_msg;

        uint8_t is_press_key = 0;
        uint8_t is_multiple_tp=0;

        double time_scale = 1;
public:
        arm_allfunction(std::string group_name, std::string baseframe_name);
        ~arm_allfunction();

        void keyCtrlCallback(const std_msgs::Int32 &msg);

        void jointStateCallback(const sensor_msgs::JointStateConstPtr  &msg);

        void setTarget(const geometry_msgs::Pose &msg);

        moveit::planning_interface::MoveItErrorCode plan();

        void execute(moveit::planning_interface::MoveGroupInterface::Plan _plan);

        void planAndExecute();
                                        
        void kernel();

        void visualizePose(geometry_msgs::Pose  _pose);

        void yamlInit(const std::string _path);

        void visualizeInit();

        void init(const std::string _path);

        void updatePose(ChgDirectionEnumdef chg_direction, PoseDimensionEnumdef pose_dismension);

        void chgPoseOrientation(ChgDirectionEnumdef chg_direction, PoseDimensionEnumdef pose_dismension);

        void printPoseInfo();

        moveit_msgs::RobotTrajectory combineTrajectory(
                                                                                                                        geometry_msgs::Pose msg1, 
                                                                                                                        geometry_msgs::Pose msg2,
                                                                                                                        moveit::planning_interface::MoveGroupInterface *move_group
                                                                                                                );

        moveit_msgs::RobotTrajectory turnPlanTime(moveit_msgs::RobotTrajectory trajectory_,  double& scale);
};


